copp\copp\copp3/formulation.rs
1//! Problem data models and builders for TOPP3/COPP3.
2//!
3//! # Notation policy (math + code)
4//! To help users map paper notation to API fields without ambiguity,
5//! this module follows a dual notation style:
6//! - mathematical definition uses KaTeX, e.g. $a_k = \dot{s}_k^2$ and $b_k = \ddot{s}_k$;
7//! - discrete implementation uses code symbols, e.g. `a[k]`, `b[k]`, `s[k]`.
8//!
9//! # Reference
10//! Wang, Y., Hu, C., Li, Y., Yu, J., Yan, J., Liang, Y., & Jin, Z. (2026).
11//! Online time-optimal trajectory planning along parametric toolpaths with strict constraint
12//! satisfaction and certifiable feasibility guarantee.
13//! *International Journal of Machine Tools and Manufacture*, 215, 104355.
14//! <https://doi.org/10.1016/j.ijmachtools.2025.104355>
15//!
16//! # Stationary-boundary modeling note
17//! This module exposes `num_stationary_max=(start,end)` on builders as a **user input upper
18//! bound** and derives effective `num_stationary` during `build_with_linearization()`.
19//! For typical users, `num_stationary_max=(1,1)` is recommended.
20//!
21//! For stationary boundaries (`a=b=0`), using zero stationary intervals is allowed, but the
22//! boundary-time model can become ill-conditioned because the regular
23//! $c=\frac{\dddot{s}}{\dot{s}}$-constant formulation degenerates near zero speed.
24//! A short boundary neighborhood modeled by $\dddot{s}$-constant stationary intervals is the
25//! practical remedy.
26
27use crate::copp::CoppObjective;
28use crate::copp::constraints::Constraints;
29use crate::diag::{CoppError, check_boundary_state_copp3_valid, check_s_interval_valid};
30use crate::robot::robot_core::{Robot, RobotBasic, RobotTorque};
31use itertools::izip;
32
33const DEFAULT_A_LINEARIZATION_FLOOR: f64 = 1E-10;
34const DEFAULT_NUM_STATIONARY_MAX: (usize, usize) = (1, 1);
35
36/// Where the third-order rows of a station take their linearization point.
37///
38/// The tangent of `1/sqrt(a)` lies below `1/sqrt(a)` for every positive anchor, so
39/// both variants keep the linearized rows an inner approximation of the nonlinear
40/// ones; they differ only in how tight that approximation is and in which `a` the
41/// rows still admit.
42///
43/// Typical usage:
44/// - Default to [`LinearizationModeTopp3::Direct`], which reproduces the historical
45/// behaviour.
46/// - [`LinearizationModeTopp3::GivenFeasibleAdaptive`] requires `a_linearization` to
47/// be the `a` of a previously solved third-order profile and that profile's `b`
48/// to be at hand; a lower-order profile discretizes `b` as
49/// `(a[k+1] - a[k]) / (2 * ds[k])` rather than through
50/// `a[k+1] = a[k] + (b[k] + b[k+1]) * ds[k]`, so its state cannot be used here.
51/// - Given those, prefer it whenever `a` spans a wide range over the window, and
52/// especially when `a` can reach the `1e-10` scale of `EPS_ZERO`. The upper and
53/// lower row of one axis differ only by an overall sign, so anchoring both at the
54/// same small `a_linearization[k]` cancels `b` and `c` and leaves
55/// `a[k] <= 3 * a_linearization[k]` -- a cap set by the anchor rather than by the
56/// jerk limit, and one that no `b` or `c` can offset. Adapting each row
57/// separately breaks the cancellation.
58/// - On a profile that stays well away from zero the two agree closely, and
59/// [`LinearizationModeTopp3::Direct`] is the cheaper of the two.
60#[derive(Clone, Copy, Debug, Default, PartialEq)]
61pub enum LinearizationModeTopp3<'a> {
62 /// Direct:
63 /// every row of a station is linearized at `a_linearization[k]`.
64 #[default]
65 Direct,
66 /// GivenFeasibleAdaptive(b_linearization):
67 /// each row is anchored where that row is nearly active at the feasible state
68 /// `(a_linearization, b_linearization)`, so a slack row is anchored high and a
69 /// tight one stays near the state. `b_linearization` must be the path
70 /// acceleration of the same profile as `a_linearization`, with the same length;
71 /// it is normally taken from a previously computed feasible third-order
72 /// profile.
73 GivenFeasibleAdaptive(&'a [f64]),
74}
75
76/// Borrowed third-order TOPP/COPP profile parts.
77///
78/// This view keeps APIs lightweight for callers that already own separate
79/// `a`/`b` slices while preserving the same semantic grouping as
80/// [`Topp3Profile`]. The tuple layout is `(a, b, num_stationary)`.
81pub type Topp3ProfileRef<'a> = (&'a [f64], &'a [f64], (usize, usize));
82
83/// Mutable borrowed third-order TOPP/COPP profile parts.
84///
85/// This view is intended for in-place profile post-processing. The tuple
86/// layout is `(a, b, num_stationary)`, where only the `a` and `b` slices are
87/// mutable and the stationary boundary counts remain immutable metadata.
88pub type Topp3ProfileMut<'a> = (&'a mut [f64], &'a mut [f64], (usize, usize));
89
90/// Node-based third-order TOPP/COPP profile.
91///
92/// The three fields are tied together by convention: `a.len() == b.len()`,
93/// and `num_stationary` describes stationary head/tail intervals on the same
94/// path grid used with those node profiles.
95#[derive(Clone, Debug, PartialEq)]
96pub struct Topp3Profile {
97 /// Node samples of $a_k = \dot{s}_k^2$.
98 pub a: Vec<f64>,
99 /// Node samples of $b_k = \ddot{s}_k$.
100 pub b: Vec<f64>,
101 /// Stationary boundary interval counts as `(head, tail)`.
102 ///
103 /// `head` counts stationary intervals at the start of the grid, and
104 /// `tail` counts stationary intervals at the end. These markers are used
105 /// by TOPP3/COPP3 interpolation and timing reconstruction.
106 pub num_stationary: (usize, usize),
107}
108
109impl Topp3Profile {
110 /// Build a third-order profile from owned parts.
111 #[inline]
112 pub fn new(a: Vec<f64>, b: Vec<f64>, num_stationary: (usize, usize)) -> Self {
113 Self {
114 a,
115 b,
116 num_stationary,
117 }
118 }
119
120 /// Borrow the profile as `(a, b, num_stationary)` parts.
121 #[inline]
122 pub fn as_parts(&self) -> Topp3ProfileRef<'_> {
123 (&self.a, &self.b, self.num_stationary)
124 }
125
126 /// Mutably borrow the profile as `(a, b, num_stationary)` parts.
127 #[inline]
128 pub fn as_parts_mut(&mut self) -> Topp3ProfileMut<'_> {
129 (&mut self.a, &mut self.b, self.num_stationary)
130 }
131
132 /// Consume the profile and return `(a, b, num_stationary)`.
133 #[inline]
134 pub fn into_parts(self) -> (Vec<f64>, Vec<f64>, (usize, usize)) {
135 (self.a, self.b, self.num_stationary)
136 }
137}
138
139#[inline(always)]
140pub(crate) fn boundary_state_is_stationary(a: f64, b: f64) -> bool {
141 a == 0.0 && b == 0.0
142}
143
144#[inline(always)]
145fn determine_num_stationary_side(a: f64, b: f64, num_stationary_max: usize) -> usize {
146 if boundary_state_is_stationary(a, b) {
147 num_stationary_max
148 } else {
149 0
150 }
151}
152
153#[inline(always)]
154fn determine_num_stationary_pair(
155 a_boundary: (f64, f64),
156 b_boundary: (f64, f64),
157 num_stationary_max: (usize, usize),
158) -> (usize, usize) {
159 (
160 determine_num_stationary_side(a_boundary.0, b_boundary.0, num_stationary_max.0),
161 determine_num_stationary_side(a_boundary.1, b_boundary.1, num_stationary_max.1),
162 )
163}
164
165/// Check that the window keeps more station intervals than its two boundary blocks.
166///
167/// Each boundary reserves `num_stationary.max(1)` intervals, so the window of
168/// `s_len` stations must satisfy
169/// `s_len - 1 > num_stationary.0.max(1) + num_stationary.1.max(1)`.
170fn check_num_station_intervals(
171 function_name: &str,
172 s_len: usize,
173 num_stationary: (usize, usize),
174) -> Result<(), CoppError> {
175 let num_intervals = s_len.saturating_sub(1);
176 let reserved = num_stationary
177 .0
178 .max(1)
179 .saturating_add(num_stationary.1.max(1));
180 if num_intervals <= reserved {
181 return Err(CoppError::InvalidInput(
182 function_name.into(),
183 format!(
184 "{num_intervals} station intervals must exceed \
185 num_stationary.0.max(1) + num_stationary.1.max(1) = {reserved} \
186 (num_stationary = {num_stationary:?})"
187 ),
188 ));
189 }
190 Ok(())
191}
192
193/// Check that an adaptive linearization state pairs one `b` with every `a`.
194fn check_linearization_mode(
195 function_name: &str,
196 a_linearization: &[f64],
197 linearization_mode: LinearizationModeTopp3<'_>,
198) -> Result<(), CoppError> {
199 match linearization_mode {
200 LinearizationModeTopp3::GivenFeasibleAdaptive(b_linearization)
201 if b_linearization.len() != a_linearization.len() =>
202 {
203 Err(CoppError::InvalidInput(
204 function_name.into(),
205 format!(
206 "b_linearization.len() = {} must equal a_linearization.len() = {}",
207 b_linearization.len(),
208 a_linearization.len()
209 ),
210 ))
211 }
212 _ => Ok(()),
213 }
214}
215
216/// Prepared TOPP3 problem view.
217///
218/// # Method identity
219/// This is the **read-only runtime view** consumed by TOPP3/COPP3 solvers after
220/// third-order constraints have been linearized.
221///
222/// # Important invariant
223/// The builder precomputes linearized jerk buffers in [`Constraints`](crate::constraints::Constraints):
224/// - `jerk_a_linear`
225/// - `jerk_max_linear`
226///
227/// and this object subsequently holds only `&Constraints` (non-mutable view).
228///
229/// # Why this works
230/// Original third-order inequality includes a nonlinear denominator term:
231/// $$
232/// \sqrt{a(s)}\left(g\_a(s) a(s) + g\_b(s) b(s) + g\_c(s) c(s) + g\_d(s)\right) \le g\_{\text{max}}(s).
233/// $$
234///
235/// Around reference `a_linearization`, it is approximated into affine form:
236/// $$
237/// \left(g\_a(s) + \frac{g\_{\text{max}}(s)}{2a_{\text{lin}}^{3/2}}\right)a + g\_b(s) b + g\_c(s) c
238/// \le \frac{3g\_{\text{max}}(s)}{2a_{\text{lin}}^{1/2}} - g\_d(s).
239/// $$
240/// where $a_{\text{lin}}$ corresponds to code input `a_linearization[k]`.
241///
242/// The affine coefficients are stored into those two buffers for downstream LP/SOCP/RA use.
243pub struct Topp3Problem<'a> {
244 pub(crate) constraints: &'a Constraints,
245 pub(crate) idx_s_start: usize,
246 pub(crate) a_linearization: &'a [f64],
247 pub(crate) a_boundary: (f64, f64),
248 pub(crate) b_boundary: (f64, f64),
249 /// Effective stationary intervals at (start, end), derived in `build_with_linearization()` from
250 /// boundary conditions and `num_stationary_max`.
251 pub(crate) num_stationary: (usize, usize),
252}
253
254/// Builder for [`Topp3Problem`](crate::solver::topp3_socp::Topp3Problem), including optional in-build linearization.
255///
256/// # Side effect notice
257/// [`build_with_linearization`](Topp3ProblemBuilder::build_with_linearization) updates
258/// cached affine linearization data inside [`Constraints`](crate::constraints::Constraints).
259/// Raw jerk constraints remain unchanged.
260///
261/// # Why mutable?
262/// Third-order jerk rows are stored in nonconvex form after
263/// [`Robot::with_axial_jerk`](crate::robot::Robot::with_axial_jerk) or
264/// [`Constraints::with_constraint_3order`](crate::constraints::Constraints::with_constraint_3order).
265/// [`build_with_linearization`](Topp3ProblemBuilder::build_with_linearization)
266/// evaluates an affine approximation around
267/// [`a_linearization`](Topp3ProblemBuilder::a_linearization) and
268/// writes the generated coefficients into the constraint
269/// cache. Rebuild the problem whenever the linearization profile changes.
270///
271/// # Example
272/// The example below builds a TOPP3 problem and lets the builder linearize the third-order constraints.
273///
274/// ```rust
275/// # fn main() -> Result<(), copp::diag::CoppError> {
276/// use copp::robot::Robot;
277/// use copp::solver::topp3_lp::Topp3ProblemBuilder;
278///
279/// let mut robot = Robot::with_capacity(2usize, 5);
280/// let s = [0.0, 0.25, 0.5, 0.75, 1.0];
281/// robot.with_s(s.as_slice())?;
282///
283/// let a_linearization = [0.0, 0.25, 0.25, 0.25, 0.0];
284/// let _problem = Topp3ProblemBuilder::new(
285/// &mut robot,
286/// 0,
287/// &a_linearization,
288/// (0.0, 0.0),
289/// (0.0, 0.0),
290/// )
291/// .build_with_linearization()?;
292/// # Ok(())
293/// # }
294/// ```
295pub struct Topp3ProblemBuilder<'a> {
296 /// Mutable constraint storage used to build linearized TOPP3 problem data.
297 pub constraints: &'a mut Constraints,
298 /// Start station index of the optimization interval.
299 pub idx_s_start: usize,
300 /// Reference profile `a[k]` used to linearize third-order constraints.
301 pub a_linearization: &'a [f64],
302 /// Boundary values of `a=(a_start, a_final)`.
303 pub a_boundary: (f64, f64),
304 /// Boundary values of `b=(b_start, b_final)`.
305 pub b_boundary: (f64, f64),
306 /// User-input upper bound of stationary intervals at (start, end).
307 pub num_stationary_max: (usize, usize),
308 /// Denominator floor for stable evaluation of `1/sqrt(a_linearization)` near `a=0`.
309 ///
310 /// Effective usage in linearization is:
311 /// $$
312 /// \frac{1}{\sqrt{\max(a_{lin}, a_{floor})}}.
313 /// $$
314 /// Discrete code form:
315 /// `1.0 / max(a_linearization, a_linearization_floor).sqrt()`.
316 ///
317 /// More details are available in the [`Topp3Problem`](crate::solver::topp3_socp::Topp3Problem) documentation.
318 pub a_linearization_floor: f64,
319
320 /// Where the third-order rows take their linearization point.
321 ///
322 /// Defaults to [`LinearizationModeTopp3::Direct`], which reproduces the historical
323 /// behaviour exactly.
324 pub linearization_mode: LinearizationModeTopp3<'a>,
325}
326
327impl<'a> Topp3ProblemBuilder<'a> {
328 /// Create a TOPP3 builder with required fields.
329 ///
330 /// Defaults:
331 /// - `num_stationary_max = (1, 1)`
332 /// - `a_linearization_floor = 1E-10`
333 pub fn new<M: RobotBasic>(
334 robot: &'a mut Robot<M>,
335 idx_s_start: usize,
336 a_linearization: &'a [f64],
337 a_boundary: (f64, f64),
338 b_boundary: (f64, f64),
339 ) -> Self {
340 Self {
341 constraints: &mut robot.constraints,
342 idx_s_start,
343 a_linearization,
344 a_boundary,
345 b_boundary,
346 num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
347 a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
348 linearization_mode: LinearizationModeTopp3::Direct,
349 }
350 }
351
352 /// Create a TOPP3 builder with required fields.
353 ///
354 /// Defaults:
355 /// - `num_stationary_max = (1, 1)`
356 /// - `a_linearization_floor = 1E-10`
357 pub fn with_constraint(
358 constraints: &'a mut Constraints,
359 idx_s_start: usize,
360 a_linearization: &'a [f64],
361 a_boundary: (f64, f64),
362 b_boundary: (f64, f64),
363 ) -> Self {
364 Self {
365 constraints,
366 idx_s_start,
367 a_linearization,
368 a_boundary,
369 b_boundary,
370 num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
371 a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
372 linearization_mode: LinearizationModeTopp3::Direct,
373 }
374 }
375
376 /// Set symmetric stationary upper bound: `num_stationary_max=(n,n)`.
377 ///
378 /// See module-level **Stationary-boundary modeling note** for guidance.
379 #[inline]
380 pub fn with_num_stationary_max(mut self, num_stationary_max: usize) -> Self {
381 self.num_stationary_max = (num_stationary_max, num_stationary_max);
382 self
383 }
384
385 /// Set asymmetric stationary upper bound: `num_stationary_max=(start,end)`.
386 ///
387 /// See module-level **Stationary-boundary modeling note** for guidance.
388 #[inline]
389 pub fn with_num_stationary_max_pair(mut self, num_stationary_max: (usize, usize)) -> Self {
390 self.num_stationary_max = num_stationary_max;
391 self
392 }
393
394 /// Set denominator floor used in third-order linearization near `a = 0`.
395 ///
396 /// [`build_with_linearization`](Self::build_with_linearization) applies
397 /// `1.0 / max(a_linearization[k], floor).sqrt()` when converting nonlinear
398 /// jerk rows into affine rows.
399 #[inline]
400 pub fn with_a_linearization_floor(mut self, floor: f64) -> Self {
401 self.a_linearization_floor = floor;
402 self
403 }
404
405 /// Choose where the third-order rows take their linearization point.
406 ///
407 /// The default is [`LinearizationModeTopp3::Direct`]. The conditions
408 /// [`LinearizationModeTopp3::GivenFeasibleAdaptive`] requires of
409 /// `a_linearization`, and when it is worth selecting over the default, are
410 /// listed under *Typical usage* on [`LinearizationModeTopp3`].
411 #[inline]
412 pub fn with_linearization_mode(
413 mut self,
414 linearization_mode: LinearizationModeTopp3<'a>,
415 ) -> Self {
416 self.linearization_mode = linearization_mode;
417 self
418 }
419
420 /// Build a TOPP3 problem and linearize third-order constraints in one step.
421 ///
422 /// This validates boundaries/interval/station count/floor first, then writes linearized jerk buffers
423 /// inside [`Constraints`](crate::constraints::Constraints). The generated rows
424 /// can be inspected with
425 /// [`Constraints::get_jerk_linear_constraints`](crate::constraints::Constraints::get_jerk_linear_constraints).
426 ///
427 /// # Rebuilding rule
428 /// If a later solver result is used as a new
429 /// [`a_linearization`](Topp3ProblemBuilder::a_linearization), create a new
430 /// builder and call [`build_with_linearization`](Topp3ProblemBuilder::build_with_linearization)
431 /// again before solving.
432 pub fn build_with_linearization(self) -> Result<Topp3Problem<'a>, CoppError> {
433 check_boundary_state_copp3_valid(self.a_boundary, self.b_boundary)?;
434 if self.a_linearization.is_empty() {
435 return Err(CoppError::InvalidInput(
436 "Topp3ProblemBuilder::build_with_linearization".into(),
437 "a_linearization cannot be empty".into(),
438 ));
439 }
440 let idx_s_final = self.idx_s_start + self.a_linearization.len() - 1;
441 check_s_interval_valid(
442 "Topp3ProblemBuilder::build_with_linearization",
443 self.idx_s_start,
444 idx_s_final,
445 )?;
446 if self.a_linearization_floor <= 0.0 {
447 return Err(CoppError::InvalidInput(
448 "Topp3ProblemBuilder::build_with_linearization".into(),
449 format!(
450 "a_linearization_floor must be positive, got {}",
451 self.a_linearization_floor
452 ),
453 ));
454 }
455
456 let num_stationary = determine_num_stationary_pair(
457 self.a_boundary,
458 self.b_boundary,
459 self.num_stationary_max,
460 );
461 check_num_station_intervals(
462 "Topp3ProblemBuilder::build_with_linearization",
463 self.a_linearization.len(),
464 num_stationary,
465 )?;
466 check_linearization_mode(
467 "Topp3ProblemBuilder::build_with_linearization",
468 self.a_linearization,
469 self.linearization_mode,
470 )?;
471
472 match self.linearization_mode {
473 LinearizationModeTopp3::Direct => {
474 self.constraints.linearize_constraint_3order_with_floor(
475 self.a_linearization,
476 self.idx_s_start,
477 self.a_linearization_floor,
478 )
479 }
480 LinearizationModeTopp3::GivenFeasibleAdaptive(b_linearization) => {
481 self.constraints.linearize_constraint_3order_adaptive(
482 self.a_linearization,
483 b_linearization,
484 self.idx_s_start,
485 self.a_linearization_floor,
486 num_stationary,
487 )
488 }
489 }
490 .map_err(|e| {
491 CoppError::InvalidInput(
492 "Topp3ProblemBuilder::build_with_linearization".into(),
493 format!("linearize_constraint_3order failed: {e}"),
494 )
495 })?;
496
497 Ok(Topp3Problem {
498 constraints: &*self.constraints,
499 idx_s_start: self.idx_s_start,
500 a_linearization: self.a_linearization,
501 a_boundary: self.a_boundary,
502 b_boundary: self.b_boundary,
503 num_stationary,
504 })
505 }
506}
507
508/// The problem of COPP3.
509/// # Arguments
510/// * `robot` - A robot with torque implemented, which defines the constraints and dynamic of the problem.
511/// * `objectives` - Objectives for COPP3 optimization.
512/// * `idx_s_start` - The starting index along the path (reached).
513/// * `a_linearization` - Linearization reference profile for third-order constraints.
514/// * `a_boundary=(a_start,a_final)` - The initial and final acceleration at the start and end of the path.
515/// * `b_boundary=(b_start,b_final)` - The initial and final boundary conditions for `b`.
516/// * `num_stationary=(start,end)` - Effective stationary intervals derived in `build_with_linearization()`.
517pub struct Copp3Problem<'a, M: RobotTorque> {
518 pub(crate) robot: &'a mut Robot<M>,
519 pub(crate) objectives: &'a [CoppObjective<'a>],
520 pub(crate) idx_s_start: usize,
521 pub(crate) a_linearization: &'a [f64],
522 pub(crate) a_boundary: (f64, f64),
523 pub(crate) b_boundary: (f64, f64),
524 /// Effective stationary intervals at (start, end), derived in `build_with_linearization()` from
525 /// boundary conditions and `num_stationary_max`.
526 pub(crate) num_stationary: (usize, usize),
527}
528
529/// Builder for [`Copp3Problem`](crate::solver::copp3_socp::Copp3Problem).
530///
531/// # Why mutable?
532/// COPP3 uses the same third-order linearization cache as TOPP3. The builder
533/// takes [`&mut Robot<M>`](crate::robot::RobotTorque) because
534/// [`build_with_linearization`](Copp3ProblemBuilder::build_with_linearization) converts nonconvex
535/// jerk rows into affine rows around [`a_linearization`](Copp3ProblemBuilder::a_linearization) and stores those rows in
536/// [`robot.constraints`](crate::robot::Robot::constraints).
537/// Rebuild the problem whenever the linearization profile changes.
538///
539/// # Example
540/// The example below builds a COPP3 problem with time and thermal objectives, including the required third-order linearization profile.
541///
542/// ```rust
543/// # fn main() -> Result<(), copp::diag::CoppError> {
544/// use copp::robot::Robot;
545/// use copp::solver::copp3_socp::{Copp3ProblemBuilder, CoppObjective};
546///
547/// let mut robot = Robot::with_capacity(2usize, 5);
548/// let s = [0.0, 0.25, 0.5, 0.75, 1.0];
549/// robot.with_s(s.as_slice())?;
550///
551/// let a_linearization = [0.0, 0.25, 0.25, 0.25, 0.0];
552/// let normalize = [1.0, 1.0];
553/// let objectives = [
554/// CoppObjective::Time(1.0),
555/// CoppObjective::ThermalEnergy(0.1, &normalize),
556/// ];
557///
558/// let _problem = Copp3ProblemBuilder::new(
559/// &mut robot,
560/// &objectives,
561/// 0,
562/// &a_linearization,
563/// (0.0, 0.0),
564/// (0.0, 0.0),
565/// )
566/// .build_with_linearization()?;
567/// # Ok(())
568/// # }
569/// ```
570pub struct Copp3ProblemBuilder<'a, M: RobotTorque> {
571 /// A robot with torque implemented, which defines the constraints and dynamic of the problem.
572 pub robot: &'a mut Robot<M>,
573 /// Objectives for COPP3 optimization.
574 pub objectives: &'a [CoppObjective<'a>],
575 /// The starting index along the path (reached).
576 pub idx_s_start: usize,
577 /// Linearization reference profile for third-order constraints.
578 pub a_linearization: &'a [f64],
579 /// `a_boundary=(a_start,a_final)` - The initial and final acceleration at the start and end of the path.
580 pub a_boundary: (f64, f64),
581 /// `b_boundary=(b_start,b_final)` - The initial and final boundary conditions for `b`.
582 pub b_boundary: (f64, f64),
583 /// User-input upper bound of stationary intervals at (start, end).
584 pub num_stationary_max: (usize, usize),
585 /// Denominator floor for stable evaluation of `1/sqrt(a_linearization)` near `a=0`.
586 ///
587 /// Effective usage in linearization is:
588 /// $$
589 /// \frac{1}{\sqrt{\max(a_{lin}, a_{floor})}}.
590 /// $$
591 /// Discrete code form:
592 /// `1.0 / max(a_linearization, a_linearization_floor).sqrt()`.
593 ///
594 /// More details are available in the [`Topp3Problem`](crate::solver::topp3_lp::Topp3Problem) documentation.
595 pub a_linearization_floor: f64,
596
597 /// Where the third-order rows take their linearization point.
598 ///
599 /// Defaults to [`LinearizationModeTopp3::Direct`], which reproduces the historical
600 /// behaviour exactly.
601 pub linearization_mode: LinearizationModeTopp3<'a>,
602}
603
604impl<'a, M: RobotTorque> Copp3ProblemBuilder<'a, M> {
605 /// Create a COPP3 builder with required fields.
606 ///
607 /// Defaults:
608 /// - `num_stationary_max = (1, 1)`
609 /// - `a_linearization_floor = 1E-10`
610 pub fn new(
611 robot: &'a mut Robot<M>,
612 objectives: &'a [CoppObjective<'a>],
613 idx_s_start: usize,
614 a_linearization: &'a [f64],
615 a_boundary: (f64, f64),
616 b_boundary: (f64, f64),
617 ) -> Self {
618 Self {
619 robot,
620 objectives,
621 idx_s_start,
622 a_linearization,
623 a_boundary,
624 b_boundary,
625 num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
626 a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
627 linearization_mode: LinearizationModeTopp3::Direct,
628 }
629 }
630
631 /// Set symmetric stationary upper bound: `num_stationary_max=(n,n)`.
632 ///
633 /// See module-level **Stationary-boundary modeling note** for guidance.
634 #[inline]
635 pub fn with_num_stationary_max(mut self, num_stationary_max: usize) -> Self {
636 self.num_stationary_max = (num_stationary_max, num_stationary_max);
637 self
638 }
639
640 /// Set asymmetric stationary upper bound: `num_stationary_max=(start,end)`.
641 ///
642 /// See module-level **Stationary-boundary modeling note** for guidance.
643 #[inline]
644 pub fn with_num_stationary_max_pair(mut self, num_stationary_max: (usize, usize)) -> Self {
645 self.num_stationary_max = num_stationary_max;
646 self
647 }
648
649 /// Set denominator floor used in third-order linearization near `a = 0`.
650 ///
651 /// [`build_with_linearization`](Self::build_with_linearization) applies
652 /// `1.0 / max(a_linearization[k], floor).sqrt()` when converting nonlinear
653 /// jerk rows into affine rows.
654 #[inline]
655 pub fn with_a_linearization_floor(mut self, floor: f64) -> Self {
656 self.a_linearization_floor = floor;
657 self
658 }
659
660 /// Choose where the third-order rows take their linearization point.
661 ///
662 /// The default is [`LinearizationModeTopp3::Direct`]. The conditions
663 /// [`LinearizationModeTopp3::GivenFeasibleAdaptive`] requires of
664 /// `a_linearization`, and when it is worth selecting over the default, are
665 /// listed under *Typical usage* on [`LinearizationModeTopp3`].
666 #[inline]
667 pub fn with_linearization_mode(
668 mut self,
669 linearization_mode: LinearizationModeTopp3<'a>,
670 ) -> Self {
671 self.linearization_mode = linearization_mode;
672 self
673 }
674
675 /// Build a validated COPP3 problem and linearize third-order constraints in one step.
676 ///
677 /// This validates boundaries/interval/station count/floor first, then writes linearized jerk buffers
678 /// inside [`Constraints`](crate::constraints::Constraints). The generated rows
679 /// can be inspected with
680 /// [`Constraints::get_jerk_linear_constraints`](crate::constraints::Constraints::get_jerk_linear_constraints).
681 ///
682 /// # Rebuilding rule
683 /// If a later solver result is used as a new [`a_linearization`](Copp3ProblemBuilder::a_linearization), create a new
684 /// builder and call [`build_with_linearization`](Copp3ProblemBuilder::build_with_linearization) again before solving.
685 pub fn build_with_linearization(self) -> Result<Copp3Problem<'a, M>, CoppError> {
686 check_boundary_state_copp3_valid(self.a_boundary, self.b_boundary)?;
687 if self.a_linearization.is_empty() {
688 return Err(CoppError::InvalidInput(
689 "Copp3ProblemBuilder::build_with_linearization".into(),
690 "a_linearization cannot be empty".into(),
691 ));
692 }
693 let idx_s_final = self.idx_s_start + self.a_linearization.len() - 1;
694 check_s_interval_valid(
695 "Copp3ProblemBuilder::build_with_linearization",
696 self.idx_s_start,
697 idx_s_final,
698 )?;
699 self.robot
700 .constraints
701 .check_s_in_bounds(self.idx_s_start, self.a_linearization.len())?;
702 if self.a_linearization_floor <= 0.0 {
703 return Err(CoppError::InvalidInput(
704 "Copp3ProblemBuilder::build_with_linearization".into(),
705 format!(
706 "a_linearization_floor must be positive, got {}",
707 self.a_linearization_floor
708 ),
709 ));
710 }
711
712 let num_stationary = determine_num_stationary_pair(
713 self.a_boundary,
714 self.b_boundary,
715 self.num_stationary_max,
716 );
717 check_num_station_intervals(
718 "Copp3ProblemBuilder::build_with_linearization",
719 self.a_linearization.len(),
720 num_stationary,
721 )?;
722 check_linearization_mode(
723 "Copp3ProblemBuilder::build_with_linearization",
724 self.a_linearization,
725 self.linearization_mode,
726 )?;
727
728 match self.linearization_mode {
729 LinearizationModeTopp3::Direct => self
730 .robot
731 .constraints
732 .linearize_constraint_3order_with_floor(
733 self.a_linearization,
734 self.idx_s_start,
735 self.a_linearization_floor,
736 ),
737 LinearizationModeTopp3::GivenFeasibleAdaptive(b_linearization) => {
738 self.robot.constraints.linearize_constraint_3order_adaptive(
739 self.a_linearization,
740 b_linearization,
741 self.idx_s_start,
742 self.a_linearization_floor,
743 num_stationary,
744 )
745 }
746 }
747 .map_err(|e| {
748 CoppError::InvalidInput(
749 "Copp3ProblemBuilder::build_with_linearization".into(),
750 format!("linearize_constraint_3order failed: {e}"),
751 )
752 })?;
753
754 Ok(Copp3Problem {
755 robot: self.robot,
756 objectives: self.objectives,
757 idx_s_start: self.idx_s_start,
758 a_linearization: self.a_linearization,
759 a_boundary: self.a_boundary,
760 b_boundary: self.b_boundary,
761 num_stationary,
762 })
763 }
764}
765
766impl<'a, M: RobotTorque> Copp3Problem<'a, M> {
767 /// Update linearization profile.
768 #[inline]
769 pub fn set_a_linearization(&mut self, a_linearization: &'a [f64]) {
770 self.a_linearization = a_linearization;
771 }
772
773 /// Backward-compatible alias for `set_a_linearization`.
774 #[inline]
775 pub fn set_a_linear(&mut self, a_linearization: &'a [f64]) {
776 self.set_a_linearization(a_linearization);
777 }
778
779 /// Update objective list.
780 #[inline]
781 pub fn set_objective(&mut self, objective: &'a [CoppObjective<'a>]) {
782 self.objectives = objective;
783 }
784
785 /// Convert to the TOPP3 view that shares interval/boundary/linearization fields.
786 pub fn as_topp3_problem(&self) -> Topp3Problem<'_> {
787 Topp3Problem {
788 constraints: &self.robot.constraints,
789 idx_s_start: self.idx_s_start,
790 a_linearization: self.a_linearization,
791 a_boundary: self.a_boundary,
792 b_boundary: self.b_boundary,
793 num_stationary: self.num_stationary,
794 }
795 }
796}
797
798/// Get the weight of `a` for value function.
799/// Time loss = \sum_{k=0}^n weight_a[k] / sqrt(a[k]) \approx Time
800pub(crate) fn get_weight_a_topp3(s: &[f64], num_stationary: (usize, usize)) -> Vec<f64> {
801 let n = s.len() - 1;
802 let mut weight_a = vec![0.0; s.len()];
803 if num_stationary.0 > 0 {
804 weight_a[num_stationary.0] =
805 0.5 * (5.0 * s[num_stationary.0] + s[num_stationary.0 + 1] - 6.0 * s[0]);
806 }
807 if num_stationary.1 > 0 {
808 weight_a[n - num_stationary.1] =
809 0.5 * (6.0 * s[n] - 5.0 * s[n - num_stationary.1] - s[n - num_stationary.1 - 1]);
810 }
811 weight_a
812 .iter_mut()
813 .skip(1)
814 .zip(s.windows(3))
815 .skip(num_stationary.0)
816 .take(n - num_stationary.0 - num_stationary.1 - 1)
817 .for_each(|(w_a, s_slice)| {
818 *w_a = 0.5 * (s_slice[2] - s_slice[0]);
819 });
820
821 weight_a
822}
823
824/// Get the weight of `a` for value function.
825/// Loss = weight[0] * loss_average_left / sqrt(a[num_stationary.0]) + weight[n] * loss_average_right / sqrt(a[n - num_stationary.1]) + \sum_{k=num_stationary.0}^{n-num_stationary.1-1} weight_a[k] * loss[k] / sqrt(a[k])
826pub(crate) fn get_weight_a_copp3(s: &[f64], num_stationary: (usize, usize)) -> Vec<f64> {
827 let n = s.len() - 1;
828 let mut weight_a = vec![0.0; s.len()];
829 if num_stationary.0 > 0 {
830 let s_n1 = s[num_stationary.0];
831 weight_a[num_stationary.0] = 0.5 * (s[num_stationary.0 + 1] - s_n1);
832 weight_a[0] = 3.0 * (s_n1 - s[0]);
833 }
834 if num_stationary.1 > 0 {
835 let s_n2 = s[n - num_stationary.1];
836 weight_a[n - num_stationary.1] = 0.5 * (s_n2 - s[n - num_stationary.1 - 1]);
837 weight_a[n] = 3.0 * (s[n] - s_n2);
838 }
839 weight_a
840 .iter_mut()
841 .skip(1)
842 .zip(s.windows(3))
843 .skip(num_stationary.0)
844 .take(n - num_stationary.0 - num_stationary.1 - 1)
845 .for_each(|(w_a, s_slice)| {
846 *w_a = 0.5 * (s_slice[2] - s_slice[0]);
847 });
848
849 weight_a
850}
851
852/// Get the `a` and `b` profiles for stationary intervals.
853pub(crate) fn set_ab_stationary_topp3<const START: bool>(
854 s: &[f64],
855 a: &mut [f64],
856 b: &mut [f64],
857 a_stationary: f64,
858 num_stationary: usize,
859) {
860 if num_stationary == 0 {
861 return;
862 }
863 if START {
864 let s_start = s[0];
865 let ds_start = s[num_stationary] - s_start;
866 *a.first_mut().unwrap() = 0.0;
867 *b.first_mut().unwrap() = 0.0;
868 a[num_stationary] = a_stationary;
869 b[num_stationary] = a_stationary / (1.5 * ds_start);
870 if num_stationary > 1 {
871 for (a_k, b_k, &s_k) in izip!(a.iter_mut(), b.iter_mut(), s.iter())
872 .skip(1)
873 .take(num_stationary - 1)
874 {
875 let dsk_start = s_k - s_start;
876 let mut alpha = dsk_start / ds_start;
877 alpha *= alpha.cbrt();
878 *a_k = a_stationary * alpha;
879 *b_k = *a_k / (1.5 * dsk_start);
880 }
881 }
882 } else {
883 let &s_final = s.last().unwrap();
884 let n = s.len() - 1;
885 let ds_final = s[n - num_stationary] - s_final;
886 *a.last_mut().unwrap() = 0.0;
887 *b.last_mut().unwrap() = 0.0;
888 a[a.len() - 1 - num_stationary] = a_stationary;
889 b[b.len() - 1 - num_stationary] = a_stationary / (1.5 * ds_final);
890 if num_stationary > 1 {
891 for (a_k, b_k, &s_k) in izip!(a.iter_mut().rev(), b.iter_mut().rev(), s.iter().rev())
892 .skip(1)
893 .take(num_stationary - 1)
894 {
895 let dsk_final = s_k - s_final;
896 let mut alpha = dsk_final / ds_final;
897 alpha *= alpha.cbrt();
898 *a_k = a_stationary * alpha;
899 *b_k = *a_k / (1.5 * dsk_final);
900 }
901 }
902 }
903}
904
905#[cfg(test)]
906mod tests {
907 use super::{
908 LinearizationModeTopp3, boundary_state_is_stationary, check_linearization_mode,
909 check_num_station_intervals, determine_num_stationary_pair,
910 };
911
912 #[test]
913 fn test_determine_num_stationary_pair_respects_boundary_state() {
914 let pair = determine_num_stationary_pair((0.0, 0.0), (0.0, 0.0), (2, 3));
915 assert_eq!(pair, (2, 3));
916
917 let pair = determine_num_stationary_pair((1.0, 0.0), (0.0, 0.0), (2, 3));
918 assert_eq!(pair, (0, 3));
919
920 let pair = determine_num_stationary_pair((0.0, 1.0), (0.0, 1.0), (2, 3));
921 assert_eq!(pair, (2, 0));
922
923 let tiny_a = f64::from_bits(1);
924 let tiny_b = f64::from_bits(2);
925 let pair = determine_num_stationary_pair((tiny_a, tiny_a), (tiny_b, tiny_b), (2, 3));
926 assert_eq!(pair, (0, 0));
927 assert!(!boundary_state_is_stationary(tiny_a, tiny_b));
928 assert!(boundary_state_is_stationary(-0.0, 0.0));
929 }
930
931 #[test]
932 fn test_check_num_station_intervals_reserves_both_boundary_blocks() {
933 // Each side reserves `num_stationary.max(1)` intervals.
934 assert!(check_num_station_intervals("test", 4, (0, 0)).is_ok());
935 assert!(check_num_station_intervals("test", 3, (0, 0)).is_err());
936 assert!(check_num_station_intervals("test", 4, (1, 1)).is_ok());
937 assert!(check_num_station_intervals("test", 3, (1, 1)).is_err());
938 assert!(check_num_station_intervals("test", 6, (1, 3)).is_ok());
939 assert!(check_num_station_intervals("test", 5, (1, 3)).is_err());
940 assert!(check_num_station_intervals("test", 0, (0, 0)).is_err());
941 assert!(check_num_station_intervals("test", usize::MAX, (usize::MAX, 1)).is_err());
942 }
943
944 #[test]
945 fn test_check_linearization_mode_requires_one_b_per_a() {
946 let a = [0.25; 5];
947 let b = [0.0; 5];
948 let b_short = [0.0; 4];
949 assert!(check_linearization_mode("test", &a, LinearizationModeTopp3::Direct).is_ok());
950 assert!(
951 check_linearization_mode(
952 "test",
953 &a,
954 LinearizationModeTopp3::GivenFeasibleAdaptive(&b)
955 )
956 .is_ok()
957 );
958 assert!(
959 check_linearization_mode(
960 "test",
961 &a,
962 LinearizationModeTopp3::GivenFeasibleAdaptive(&b_short)
963 )
964 .is_err()
965 );
966 }
967}